package monitor;

import i_grafica.CarnadaGUI;
import i_grafica.botonesd;

import java.io.IOException;
import java.util.Iterator;

import javax.vecmath.Point3d;

import estrategia.Controlador;
import estrategia.Utiles;

/**
 * 
 * Estaria bueno que las demas operaciones logicas, tuvieran la opcion de pausarse
 * de esta manera, el algoritmo puede cobrar relevancia frente a las otras cosas 
 * 
 * */
public class Salird extends Thread{

	private boolean vivo = true;
	private Point3d mi_posicion;
	private Point3d posicion_otros[];
	private Controlador control;
	private Utiles u;
	
	/*seters*/
	public void killThread() { vivo = false; }

	/*geters*/
	public boolean estaVivo() { return vivo; }
	
	private void colisionPared() throws IOException{
		
		//Obtengo posicion de mi robot.
		double distancia_pared_delantera = Constantes.POSICION_PARED_DELANTERA - mi_posicion.y; 
		double distancia_pared_trasera =  mi_posicion.y - Constantes.POSICION_PARED_TRASERA;
		double distancia_pared_izquierda =  mi_posicion.x - Constantes.POSICION_PARED_IZQUIERDA;
		double distancia_pared_derecha = Constantes.POSICION_PARED_DERECHA - mi_posicion.x;
			
		//Si estoy muy proximo a la pared, y perpendicular, salgo.
		
		if ( distancia_pared_delantera < Constantes.TOLERANCIA_PARED && angulosIguales(mi_posicion.z, Math.PI/2)||
				distancia_pared_trasera < Constantes.TOLERANCIA_PARED && angulosIguales(mi_posicion.z, 3*Math.PI/2) ||
				distancia_pared_izquierda < Constantes.TOLERANCIA_PARED && angulosIguales(mi_posicion.z, Math.PI) ||
				distancia_pared_derecha < Constantes.TOLERANCIA_PARED  && angulosIguales(mi_posicion.z, 0)) {
			
			control.entorno.enviarBeep();
			
			//Pauso tactica actual										
			pararDemonios();
			
			//Retrocedo
			control.retroceder(Constantes.VELOCIDAD_ESCAPE);
			try {
				sleep(Constantes.TIEMPO_DE_RETROCESO);
			} catch (InterruptedException e) {
				// TODO Auto-generated catch block
				e.printStackTrace();
			}
			control.detenerMotores();
			//Giro
			double angulo_giro = (mi_posicion.z + Constantes.ANGULO_ESCAPE) % 2*Math.PI;
			control.girar(angulo_giro);
		}
	}
	
	/**
	 * Devuelve si 2 angulos son iguales, tomando en cuenta cierto margen 
	 * */
	public static boolean angulosIguales(double angulo1, double angulo2) {
		angulo1 = angulo1 % (2*Math.PI);
		angulo2 = angulo2 % (2*Math.PI);
		double distancia_cos = Math.abs(Math.cos(angulo1) - Math.cos(angulo2));
		double distancia_sin = Math.abs(Math.sin(angulo1) - Math.sin(angulo2)) ;
		if( distancia_cos < Constantes.TOLERANCIA_ANGULOS_OPUESTOS_COS &&
				distancia_sin < Constantes.TOLERANCIA_ANGULOS_OPUESTOS_SIN)
			return true;
		
		return false;
	}
	
	public static double normalizarAngulo(double angulo) {
		double res = angulo % (2*Math.PI);
		while( res < 0 ) {
			res = res + (2*Math.PI);
		}
		return res;
	}
			
	private void colisionFrontalRobot() throws IOException{
		
		for (int i = 0; i < Constantes.enemigos.length; i++) {
			Point3d enemigo_actual = posicion_otros[Constantes.enemigos[i]];
						
			if(enemigo_actual != null){
				
				double distancia_al_enemigo = u.distanciaEuclidea(mi_posicion,enemigo_actual);  
				
			
				if( distancia_al_enemigo < Constantes.TOLERANCIA_ENEMIGOS ) {					
					System.out.println("Entre en la tolerancia");					
				}
				
				double mi_angulo = normalizarAngulo(mi_posicion.z);
								
				if( distancia_al_enemigo < Constantes.TOLERANCIA_ENEMIGOS &&
						angulosIguales(mi_posicion.z + Math.PI, enemigo_actual.z)) {
					
					if( (mi_posicion.y < enemigo_actual.y && mi_angulo >= 0 && mi_angulo < Math.PI )||
							(mi_posicion.y > enemigo_actual.y && mi_angulo < (2*Math.PI) && mi_angulo >= Math.PI )) {
											
						control.entorno.enviarBeep();
						
						//Pauso tactica actual										
						pararDemonios();
						
						//Retrocedo
						control.retroceder(Constantes.VELOCIDAD_ESCAPE);
						try {
							sleep(Constantes.TIEMPO_DE_RETROCESO);
						} catch (InterruptedException e) {
							// TODO Auto-generated catch block
							e.printStackTrace();
						}
						control.detenerMotores();
						//Giro
						double angulo_giro = (mi_posicion.z + Constantes.ANGULO_ESCAPE) % (2*Math.PI);
						control.girar(angulo_giro);
					}
				}
			} 
		}
		
	}
	
	private void pararDemonios() {
		Iterator<botonesd> iteradorB = CarnadaGUI.listaDaemons.iterator();
		while(iteradorB.hasNext()) {			
			botonesd thread = iteradorB.next();
			iteradorB.remove();
			thread.stop();
		}		
	}
	
	private void colisionFuerte() throws IOException{
		
		for (int i = 0; i < Constantes.fuertes.length; i++) {
			Point3d fuerte_actual = posicion_otros[Constantes.fuertes[i]];
						
			double distancia_al_fuerte = u.distanciaEuclidea(mi_posicion,fuerte_actual);
			
			if( distancia_al_fuerte < Constantes.TOLERANCIA_FUERTES ) {
				
				control.entorno.enviarBeep();
				
				//Pauso tactica actual										
				pararDemonios();
				
				//Retrocedo
				control.retroceder(Constantes.VELOCIDAD_ESCAPE);
				try {
					sleep(Constantes.TIEMPO_DE_RETROCESO);
				} catch (InterruptedException e) {
					// TODO Auto-generated catch block
					e.printStackTrace();
				}
				control.detenerMotores();
				//Giro
				double angulo_giro = (mi_posicion.z + Constantes.ANGULO_ESCAPE) % (2*Math.PI);
				control.girar(angulo_giro);
			}
		}		
	}
	
	public Salird(Controlador c){		
		control = c;
		u = new Utiles(control.entorno);
	}
	
	
	
	@Override
	public void run() {
		
		while (vivo) {			
			try {
				mi_posicion = control.getMiPosicion();
				posicion_otros = control.getPosicionesObstaculos();
				//colisionPared();
				//colisionFuerte();
				//colisionFrontalRobot();
				sleep(Constantes.TIEMPO_ENTRE_CONTROLES);
			} catch (IOException e) {
				// TODO Auto-generated catch block
				e.printStackTrace();
			} catch (InterruptedException e) {
				// TODO Auto-generated catch block
				e.printStackTrace();
			}
		}
	}
}
